#!/usr/bin/python
import cwiid
import sys
import roslib; roslib.load_manifest('wii_balance')
import rospy
from std_msgs.msg import Float32MultiArray
from time import sleep

current_measurement = [0]*4;
current_raw = [0]*4;
def main():
	global named_calibration
	rpt_mode = 0
	mesg = True
	wiimote = cwiid.Wiimote()
	rpt_mode = cwiid.RPT_EXT | cwiid.FLAG_CONTINUOUS
	wiimote.rpt_mode = rpt_mode
	wiimote.enable(cwiid.FLAG_MESG_IFC);
	#publish ros service

	#get calibration information
	wiimote.request_status()
	balance_calibration = wiimote.get_balance_cal()
	named_calibration = {   'right_top': balance_calibration[0],
			            	'right_bottom': balance_calibration[1],
							'left_top': balance_calibration[2],
							'left_bottom': balance_calibration[3],
						}
	#Initialize talker
	pub = rospy.Publisher('chatter', Float32MultiArray)
	rospy.init_node('talker', anonymous=True)
	r = rospy.Rate(10) # 10hz

	#invoke callback
	wiimote.mesg_callback = callback
	while not rospy.is_shutdown():
		#read result from current_measurement
		#send ros message
		pub.publish(data=current_measurement) 
		pub.publish(data=current_raw)
		r.sleep()
	wiimote.close()
def callback(mesg_list, time):
	print mesg_list
	for mesg in mesg_list:
		if mesg[0] ==  cwiid.MESG_BALANCE:
			#do calibration 
			i=0
			for sensor in ('left_bottom', 'right_top', 'right_bottom', 'left_top'):
				reading = mesg[1][sensor]
				calibration = named_calibration[sensor]
				if reading < calibration[1]:
					current_measurement[i] = (1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]))/100.0
				else:
					current_measurement[i] = (1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700)/100.0
				current_raw[i] = reading
				i+=1
		elif mesg[0] ==  cwiid.MESG_ERROR:
			print "Error message received"
			global wiimote
			wiimote.close()
			exit(-1)
		else:
			print 'Unknown Report'

	return 
main()
